namespace Microsoft.Robotics.Navigation.Runtime.Motion
{
    using System;
    using System.Diagnostics;
    using System.Drawing;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Runtime.Motion;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Runtime.Cameras;

    /// <summary>
    /// Implements low level suggestion drive intended to run on the robot
    /// </summary>
    [DataContract]
    public class Ev4LocalSuggestionDriveAgent : ConsumerProducerAgent<MobileBaseVelocityReferenceMessage, MobileBaseVelocityReferenceMessage>,
                                        ISubscriptionReceiver<DepthCameraAgentMessage<KinectDepthCameraParameters>>,
                                        ISubscriptionReceiver<SonarMessage> 
    {
        /// <summary>
        /// Name of agent producing kinect raw messages.
        /// </summary>
        [DataMember(Name = "KinectRawMessageProducer")]
        private readonly string kinectProducerAgentName = default(string);

        /// <summary>
        /// Name of producer of sonar data
        /// </summary>
        [DataMember(Name = "SonarProducer")]
        private string sonarProducerAgentName = default(string);

        /// <summary>
        /// Minimum absolute escape rotation velocity. Gaussian start point.
        /// </summary>
        [DataMember(Name = "EscapeRotationRadsPerSecMinimum")]
        private double escapeRotationRadsPerSecMin;

        /// <summary>
        /// Maximum absolute escape rotation velocity. 
        /// </summary>
        [DataMember(Name = "EscapeRotationRadsPerSecMaximum")]
        private double escapeRotationRadsPerSecMax;

        /// <summary>
        /// Escape rotation variance. Will be used to sample from the gaussian.
        /// </summary>
        [DataMember(Name = "EscapeRotationRadsPerSecVariance")]
        private double escapeRotationRadsPerSecVariance;

        /// <summary>
        /// Probability with which we will respect suggestion from the upstream component on the direction of escaping an obstacle
        /// </summary>
        [DataMember(Name = "ProbabilityOfRespectingSuggestedDirection")]
        private double probabilityOfRespectingSuggestedDirection;

        /// <summary>
        /// When avoiding obstacles, we capture and then release control with a little delay intended to steer robot away by a safe amount
        /// Note! Too much (i.e. 1000) may result in oscillation while going through narrow spaces, too little may result in too much 'grazing' of the sides of obstacles
        /// </summary>
        [DataMember(Name = "OvershootWhenTurningAwayFromObstacleInMilliseconds")]
        private int desiredOverShootMillisecondsWhileTurning;

        /// <summary>
        /// Sonar distance threshold in meters. Closer than this will trigger OA.
        /// </summary>
        [DataMember(Name = "SonarThresholdDistanceInMM")]
        private int sonarThresholdDistanceInMM;

        /// <summary>
        /// Depth sensor threshold distance in millimeters. Closer than this will trigger OA.
        /// </summary>
        [DataMember(Name = "DepthThresholdInMM")]
        private int depthThresholdInMM;

        /// <summary>
        /// After avoiding an obstacle we drive forward some random amount of time. This is the max of that random.
        /// </summary>
        [DataMember(Name = "MaxMillisecondsDrivingForwardAfterTurning")]
        private int maxMillisecondsDrivingForwardAfterTurning;

        /// <summary>
        /// Speed at which we will be driving straight after avoiding an obstacle.
        /// </summary>
        [DataMember(Name = "DriveForwardSpeed")]
        private double driveForwardSpeed;

        /// <summary>
        /// Agent producing kinect messages.
        /// </summary>
        private IAgentHost kinectProducerAgent;

        /// <summary>
        /// Agent producing sonar data
        /// </summary>
        private IAgentHost sonarAgent;

        /// <summary>
        /// Suggestion drive utility instance
        /// </summary>
        private Ev4LocalSuggestionDrive suggestionDrive;

        /// <summary>
        /// State of the obstacle avoidance
        /// </summary>
        private Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState obstacleAvoidanceState;        

        /// <summary>
        /// Initializes a new instance of the Ev4LocalSuggestionDriveAgent class
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="producer">Producer agent name</param>
        /// <param name="kinectProducerAgent">Name of agent producing kinect messages.</param>
        /// <param name="sonarProducerAgent">Name of producer of sonar data</param>
        /// <param name="sonarThresholdDistanceInMM">Sonar minimum threshold distance.</param>
        /// <param name="depthThresholdInMM">Kinect minimum threshold distance.</param>
        /// <param name="escapeRotationRadsPerSecMin">Escape rotation minimum.</param>
        /// <param name="escapeRotationRadsPerSecMax">Maximum absolute escape rotation velocity.</param>
        /// <param name="escapeRotationRadsPerSecVariance">Escape rotation variance. Will be used to sample from the gaussian.</param>
        /// <param name="desiredOverShootMillisecondsWhileTurning">When turning away from an obstacle to avoid it - we overshoot to ensure we have clearance before driving straight</param>
        /// <param name="maxMillisecondsDrivingForwardAfterTurning">Amount of time to over shoot while turning away from obstacle</param>
        /// <param name="driveForwardSpeedAfterAvoiding">Speed at which we will be driving straight after avoiding an obstacle.</param>
        /// <param name="probabilityOfRespectingSuggestedDirection">Probability with which we will respect suggestion from the upstream component on the direction of escaping an obstacle.</param>
        public Ev4LocalSuggestionDriveAgent(
            string name, 
            string producer,
            IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> kinectProducerAgent,
            IProducer<SonarMessage> sonarProducerAgent,
            int sonarThresholdDistanceInMM,
            int depthThresholdInMM,
            double escapeRotationRadsPerSecMin,
            double escapeRotationRadsPerSecMax,
            double escapeRotationRadsPerSecVariance,
            int desiredOverShootMillisecondsWhileTurning,
            int maxMillisecondsDrivingForwardAfterTurning,
            double driveForwardSpeedAfterAvoiding,
            double probabilityOfRespectingSuggestedDirection)
            : base(name, producer)
        {
            this.kinectProducerAgentName = kinectProducerAgent.Name;
            this.sonarProducerAgentName = sonarProducerAgent.Name;
            this.sonarThresholdDistanceInMM = sonarThresholdDistanceInMM;
            this.depthThresholdInMM = depthThresholdInMM;
            this.escapeRotationRadsPerSecMin = escapeRotationRadsPerSecMin;
            this.escapeRotationRadsPerSecMax = escapeRotationRadsPerSecMax;
            this.escapeRotationRadsPerSecVariance = escapeRotationRadsPerSecVariance;
            this.desiredOverShootMillisecondsWhileTurning = desiredOverShootMillisecondsWhileTurning;
            this.maxMillisecondsDrivingForwardAfterTurning = maxMillisecondsDrivingForwardAfterTurning;
            this.driveForwardSpeed = driveForwardSpeedAfterAvoiding;
            this.probabilityOfRespectingSuggestedDirection = probabilityOfRespectingSuggestedDirection;
        }

        /// <summary>
        /// Called when agent is initialized
        /// </summary>
        /// <param name="locator">An agent locator</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            this.kinectProducerAgent = locator.GetAgent(this.kinectProducerAgentName);

            this.sonarAgent = locator.GetAgent(this.sonarProducerAgentName);

            this.suggestionDrive = new Ev4LocalSuggestionDrive(
                this.sonarThresholdDistanceInMM, 
                this.depthThresholdInMM, 
                this.desiredOverShootMillisecondsWhileTurning,
                this.maxMillisecondsDrivingForwardAfterTurning,
                this.driveForwardSpeed,
                this.probabilityOfRespectingSuggestedDirection);

            this.obstacleAvoidanceState = new Ev4LocalSuggestionDrive.LocalObstacleAvoidanceState(
                this.escapeRotationRadsPerSecMin,
                this.escapeRotationRadsPerSecMax,
                this.escapeRotationRadsPerSecVariance);
        }

        /// <summary>
        /// Called when the agent is activated
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();

            this.kinectProducerAgent.Subscribe<DepthCameraAgentMessage<KinectDepthCameraParameters>>(this);
            this.kinectProducerAgent.Activate();

            this.sonarAgent.Subscribe<SonarMessage>(this);
            this.sonarAgent.Activate();
        }

        /// <summary>
        /// Called when the agent is deactivated.
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();

            this.kinectProducerAgent.Deactivate();
            this.sonarAgent.Deactivate();
        }

        /// <summary>
        /// Store depth frame cheaply. We will process it only when drive command arrives, which is much cheaper than doing it
        /// greedily
        /// </summary>
        /// <param name="message">Kinect message.</param>
        public void Receive(DepthCameraAgentMessage<KinectDepthCameraParameters> message)
        {
            this.obstacleAvoidanceState.LastDepthImage = message.ImageFrame.ImageData;
            this.obstacleAvoidanceState.LastDepthImageSize = new System.Drawing.Size(message.ImageFrame.Width, message.ImageFrame.Height);
        }

        /// <summary>
        /// Store depth frame cheaply. We will process it only when drive command arrives
        /// </summary>
        /// <param name="message">Message containing sonar states</param>
        public void Receive(SonarMessage message)
        {
            this.obstacleAvoidanceState.LastSonarState = message.State;
        }

        /// <summary>
        /// Process MobileBaseVelocityReferenceMessage messages.
        /// </summary>
        /// <param name="message">MobileBaseVelocityReferenceMessage message.</param>
        public override void Receive(MobileBaseVelocityReferenceMessage message)
        {
            long freq = Stopwatch.Frequency;
            this.obstacleAvoidanceState.CurrentTimeStampInMS = (1000 * Stopwatch.GetTimestamp()) / freq;

            MobileBaseVelocityReference safeVelocity = this.suggestionDrive.SafeSpeedFromSuggestedSpeed(
                this.obstacleAvoidanceState,
                message.MobileBaseVelocityReference);

            this.Publisher.Post(new MobileBaseVelocityReferenceMessage(safeVelocity, message));                        
        }
    }
}
